/**
 * @file simpleBLEPeripheral.c
 * @author chipsea
 * @brief 
 * @version 0.1
 * @date 2020-11-30
 * @copyright Copyright (c) 2020, CHIPSEA Co., Ltd.
 * @note
 */
#include "sdk_config.h"
#include "ll.h"
#include "hci.h"
#include "OSAL.h"
#include "gatt.h"
#include "gpio.h"
#include "pwrmgr.h"
#include "ll_def.h"
#include "hci_tl.h"
#include "bcomdef.h"
#include "ll_hw_drv.h"
#include "gapbondmgr.h"
#include "peripheral.h"
#include "OSAL_PwrMgr.h"
#include "gattservapp.h"
#include "rf_phy_driver.h"
#include "global_config.h"
#include "gapgattserver.h"
#include "sbpProfile_ota.h"
//#include "devinfoservice.h"
#include "simpleBLEPeripheral.h"
#include "gpio.h"

#define WAKEUP_PIN			GPIO_P07
///< How often to perform periodic event
#define SBP_PERIODIC_EVT_PERIOD             1000

#define DEVINFO_SYSTEM_ID_LEN				8
#define DEVINFO_SYSTEM_ID					0
 
#define DEFAULT_DISCOVERABLE_MODE			GAP_ADTYPE_FLAGS_GENERAL

///< Default passcode
#define DEFAULT_PASSCODE					0

///< Length of bd addr as a string
#define B_ADDR_STR_LEN						15

#define RESOLVING_LIST_ENTRY_NUM			10

///< build define
#define APP_CFG_RPA_TEST					0

/*********************************************************************
 * GLOBAL VARIABLES
 */
perStatsByChan_t g_perStatsByChanTest;

/*********************************************************************
 * EXTERNAL VARIABLES
 */
volatile uint8_t g_current_advType = LL_ADV_CONNECTABLE_UNDIRECTED_EVT;

//extern wtnrTest_t wtnrTest;
extern l2capSARDbugCnt_t g_sarDbgCnt;
extern uint32 g_osal_mem_allo_cnt;
extern uint32 g_osal_mem_free_cnt;

extern uint32 counter_tracking;

extern uint32 g_counter_traking_avg;
extern uint32 g_counter_traking_cnt;
extern uint32_t  g_TIM2_IRQ_TIM3_CurrCount;
extern uint32_t  g_TIM2_IRQ_to_Sleep_DeltTick;
extern uint32_t  g_osal_tick_trim;
extern uint32_t  g_TIM2_IRQ_PendingTick;
extern uint32_t  g_TIM2_wakeup_delay;

/*********************************************************************
 * EXTERNAL FUNCTIONS
 */


/*********************************************************************
 * LOCAL VARIABLES
 */
static uint8 simpleBLEPeripheral_TaskID;   ///< Task ID for internal task/event processing

static gaprole_States_t gapProfileState = GAPROLE_INIT;
static uint8_t notifyBuf[256];
static uint16 notifyInterval;
static uint8 notifyPktNum;
static uint8 connEvtEndNotify;
static uint16 notifyCnt;
static uint32 notifyByteCnt;
static uint32 writeByteCnt;

#if(APP_CFG_RPA_TEST==1)
static uint8  peerIrkList[RESOLVING_LIST_ENTRY_NUM][LL_ENC_IRK_LEN];  
static uint8  localIrkList[RESOLVING_LIST_ENTRY_NUM][LL_ENC_IRK_LEN];  
static uint8  peerAddrList[RESOLVING_LIST_ENTRY_NUM][LL_DEVICE_ADDR_LEN];  
static uint8  peerAddrType[RESOLVING_LIST_ENTRY_NUM];  
#endif


///< GAP - SCAN RSP data (max size = 31 bytes)
//static uint8 scanRspData[] =
//{
//    ///< connection interval range
//    0x05,	///< length of this data
//    GAP_ADTYPE_SLAVE_CONN_INTERVAL_RANGE,
//    LO_UINT16( DEFAULT_DESIRED_MIN_CONN_INTERVAL ),	///< 100ms
//    HI_UINT16( DEFAULT_DESIRED_MIN_CONN_INTERVAL ),
//    LO_UINT16( DEFAULT_DESIRED_MAX_CONN_INTERVAL ),	///< 1s
//    HI_UINT16( DEFAULT_DESIRED_MAX_CONN_INTERVAL ),

//    ///< Tx power level
//    0x02,	///< length of this data
//    GAP_ADTYPE_POWER_LEVEL,
//    0		///< 0dBm
//};


///< advert data for iBeacon
static uint8 advertData[] =
{	
	0x02,	///< length of this data
	GAP_ADTYPE_FLAGS,
	DEFAULT_DISCOVERABLE_MODE | GAP_ADTYPE_FLAGS_BREDR_NOT_SUPPORTED,
    0x1A, // length of this data including the data type byte
    GAP_ADTYPE_MANUFACTURER_SPECIFIC, // manufacturer specific adv data type
    0x4c, // Company ID - Fixed
    0x00, // Company ID - Fixed
    0x02, // Data Type - Fixed
    0x15, // Data Length - Fixed
    DEFAULT_IBEACON_UUID ,
    HI_UINT16(DEFAULT_IBEACON_MAJOR), 	// Major
    LO_UINT16(DEFAULT_IBEACON_MAJOR), 	// Major
    HI_UINT16(DEFAULT_IBEACON_MINOR),	// Minor
    LO_UINT16(DEFAULT_IBEACON_POWER),	// Minor
    DEFAULT_IBEACON_POWER // Power - The 2's complement of the calibrated Tx Power
};


static uint8 otaAdvIntv         = 100;	///< unit is 5ms
static uint8 otaConnIntvMax     = DEFAULT_DESIRED_MIN_CONN_INTERVAL>>2;	///< unit is 5ms
static uint8 otaConnIntvMin     = DEFAULT_DESIRED_MAX_CONN_INTERVAL>>2;	///< uiit is 5ms
static uint8 otaConnIntvLatency = DEFAULT_DESIRED_SLAVE_LATENCY;		///< 
static uint8 otaConnTimeOut     = DEFAULT_DESIRED_CONN_TIMEOUT/100;		///< unit is second


///< GAP GATT Attributes
static uint8 attDeviceName[GAP_DEVICE_NAME_LEN] = DEVICE_NAME;

/*********************************************************************
 * LOCAL FUNCTIONS
 */
static void simpleBLEPeripheral_ProcessOSALMsg( osal_event_hdr_t *pMsg );
static void peripheralStateNotificationCB( gaprole_States_t newState );
static void simpleProfileChangeCB( uint8 paramID, uint8 len );
static void peripheralStateReadRssiCB( int8 rssi  );
static void updateAdvData(void);

#if(APP_CFG_RPA_TEST==1)
static void initResolvingList(void);
#endif

void check_PerStatsProcess(void);

char *bdAddr2Str( uint8 *pAddr );
//static uint8_t simpleBLEPeripheral_ScanRequestFilterCBack(void);
/*********************************************************************
 * PROFILE CALLBACKS
 */

///< GAP Role Callbacks
static gapRolesCBs_t simpleBLEPeripheral_PeripheralCBs =
{
    peripheralStateNotificationCB,	///< Profile State Change Callbacks
    peripheralStateReadRssiCB		///< When a valid RSSI is read from controller (not used by application)
};
#if (DEFAULT_GAPBOND_MGR_ENABLE==1)
//GAP Bond Manager Callbacks, add 2017-11-15
static gapBondCBs_t simpleBLEPeripheral_BondMgrCBs =
{
  NULL,		///< Passcode callback (not used by application)
  NULL		///< Pairing / Bonding state Callback (not used by application)
};
#endif
// Simple GATT Profile Callbacks
static simpleProfileCBs_t simpleBLEPeripheral_SimpleProfileCBs =
{
    simpleProfileChangeCB	///< Charactersitic value change callback
};

/*********************************************************************
 * PUBLIC FUNCTIONS
 */

/**
 * @fn		void SimpleBLEPeripheral_Init( uint8 task_id )
 * @brief	Initialization function for the Simple BLE Peripheral App Task.
 *          This is called during initialization and should contain
 *          any application specific initialization (ie. hardware
 *          initialization/setup, table initialization, power up
 *          notificaiton ... ).
 * @param	task_id - the ID assigned by OSAL.  This ID should be
 *                    used to send messages and set timers.
 * @return	none
 */
void SimpleBLEPeripheral_Init( uint8 task_id )
{
	simpleBLEPeripheral_TaskID = task_id;

	///< Setup the GAP
	VOID GAP_SetParamValue( TGAP_CONN_PAUSE_PERIPHERAL, DEFAULT_CONN_PAUSE_PERIPHERAL );

	///< Setup the GAP Peripheral Role Profile
	{
		///< device starts advertising upon initialization
		uint8 initial_advertising_enable = FALSE;

		uint8 enable_update_request = DEFAULT_ENABLE_UPDATE_REQUEST;
		uint8 advChnMap = GAP_ADVCHAN_37 | GAP_ADVCHAN_38 | GAP_ADVCHAN_39; 
		
		///< By setting this to zero, the device will go into the waiting state after
		///< being discoverable for 30.72 second, and will not being advertising again
		///< until the enabler is set back to TRUE
		uint16 gapRole_AdvertOffTime = 0;

		uint16 desired_min_interval = DEFAULT_DESIRED_MIN_CONN_INTERVAL;
		uint16 desired_max_interval = DEFAULT_DESIRED_MAX_CONN_INTERVAL;
		uint16 desired_slave_latency = DEFAULT_DESIRED_SLAVE_LATENCY;
		uint16 desired_conn_timeout = DEFAULT_DESIRED_CONN_TIMEOUT;
		
		uint8 peerPublicAddr[] = {
			0x01,
			0x02,
			0x03,
			0x04,
			0x05,
			0x06
		};

		uint8 advType =g_current_advType;	///< it seems a  bug to set GAP_ADTYPE_ADV_NONCONN_IND = 0x03
		GAPRole_SetParameter( GAPROLE_ADV_EVENT_TYPE, sizeof( uint8 ), &advType );
		
		GAPRole_SetParameter(GAPROLE_ADV_DIRECT_ADDR, sizeof(peerPublicAddr), peerPublicAddr);
		///< set adv channel map
		GAPRole_SetParameter(GAPROLE_ADV_CHANNEL_MAP, sizeof(uint8), &advChnMap);        

		///< Set the GAP Role Parameters
		GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &initial_advertising_enable );
		GAPRole_SetParameter( GAPROLE_ADVERT_OFF_TIME, sizeof( uint16 ), &gapRole_AdvertOffTime );
		
		uint8  advData[B_MAX_ADV_LEN];
		uint8 offset;
		//set scan_rsp information
		offset = 0;
		GAPRole_EncodeAdvDeviceAddr(advData, &offset, B_MAX_ADV_LEN);
		GAPRole_EncodeAdvDeviceName(advData, &offset, B_MAX_ADV_LEN, attDeviceName, osal_strlen((char *)attDeviceName));
		//osal_memcpy(advData+offset, scanRspData, sizeof(scanRspData));
		//offset += sizeof ( scanRspData );
		GAPRole_SetParameter( GAPROLE_SCAN_RSP_DATA, offset, advData );
		
		//set adv_data information
		GAPRole_SetParameter( GAPROLE_ADVERT_DATA, sizeof(advertData), advertData );

		GAPRole_SetParameter( GAPROLE_PARAM_UPDATE_ENABLE, sizeof( uint8 ), &enable_update_request );
		GAPRole_SetParameter( GAPROLE_MIN_CONN_INTERVAL, sizeof( uint16 ), &desired_min_interval );
		GAPRole_SetParameter( GAPROLE_MAX_CONN_INTERVAL, sizeof( uint16 ), &desired_max_interval );
		GAPRole_SetParameter( GAPROLE_SLAVE_LATENCY, sizeof( uint16 ), &desired_slave_latency );
		GAPRole_SetParameter( GAPROLE_TIMEOUT_MULTIPLIER, sizeof( uint16 ), &desired_conn_timeout );
	}

	///< Set the GAP Characteristics
	GGS_SetParameter( GGS_DEVICE_NAME_ATT, GAP_DEVICE_NAME_LEN, attDeviceName );

    ///< Set advertising interval
    {
        uint16 advInt = DEFAULT_DESIRED_ADV_INTERVAL;	///< actual time = advInt * 625us

        GAP_SetParamValue( TGAP_LIM_DISC_ADV_INT_MIN, advInt );
        GAP_SetParamValue( TGAP_LIM_DISC_ADV_INT_MAX, advInt );
        GAP_SetParamValue( TGAP_GEN_DISC_ADV_INT_MIN, advInt );
        GAP_SetParamValue( TGAP_GEN_DISC_ADV_INT_MAX, advInt );
    }
#if(DEFAULT_GAPBOND_MGR_ENABLE==1)
    // Setup the GAP Bond Manager, add 2017-11-15
    {
        uint32 passkey = DEFAULT_PASSCODE;
        uint8 pairMode = GAPBOND_PAIRING_MODE_WAIT_FOR_REQ;
        uint8 mitm = TRUE;
        uint8 ioCap = GAPBOND_IO_CAP_NO_INPUT_NO_OUTPUT;
        uint8 bonding = TRUE;
        GAPBondMgr_SetParameter( GAPBOND_DEFAULT_PASSCODE, sizeof ( uint32 ), &passkey );
        GAPBondMgr_SetParameter( GAPBOND_PAIRING_MODE, sizeof ( uint8 ), &pairMode );
        GAPBondMgr_SetParameter( GAPBOND_MITM_PROTECTION, sizeof ( uint8 ), &mitm );
        GAPBondMgr_SetParameter( GAPBOND_IO_CAPABILITIES, sizeof ( uint8 ), &ioCap );
        GAPBondMgr_SetParameter( GAPBOND_BONDING_ENABLED, sizeof ( uint8 ), &bonding );
    }
#endif
    // Initialize GATT attributes
    GGS_AddService( GATT_ALL_SERVICES );            // GAP
    GATTServApp_AddService( GATT_ALL_SERVICES );    // GATT attributes
    //DevInfo_AddService();                           // Device Information Service
    SimpleProfile_AddService( GATT_ALL_SERVICES );	///< Simple GATT Profile

    ///< Setup the SimpleProfile Characteristic Values
    {
        uint8  uuid_setting[IBEACON_UUID_LEN] = {
          DEFAULT_IBEACON_UUID
        };
        uint16 major = DEFAULT_IBEACON_MAJOR;
        uint16 minor = DEFAULT_IBEACON_MINOR;
        uint8 power = DEFAULT_IBEACON_POWER;
        uint8 reset[IBEACON_ATT_LONG_PKT];

        for(uint8 i=0;i<IBEACON_ATT_LONG_PKT;i++)
        {
            reset[i]=(i<6) ? 0 : i;
	    }
        SimpleProfile_SetParameter( SIMPLEPROFILE_CHAR1, IBEACON_UUID_LEN, uuid_setting);
        SimpleProfile_SetParameter( SIMPLEPROFILE_CHAR2, sizeof ( uint16 ), &major );
        SimpleProfile_SetParameter( SIMPLEPROFILE_CHAR3, sizeof ( uint16 ), &minor );
        SimpleProfile_SetParameter( SIMPLEPROFILE_CHAR4, sizeof ( uint8 ), &power );
        SimpleProfile_SetParameter( SIMPLEPROFILE_CHAR5, IBEACON_ATT_LONG_PKT, &reset );
    }

    ///< intial notifyBuf
    for(int i =0 ;i<255;i++)
        notifyBuf[i]=i;

    ///< Register callback with SimpleGATTprofile
    VOID SimpleProfile_RegisterAppCBs( &simpleBLEPeripheral_SimpleProfileCBs );
    #if (1)
    {
        uint8_t mtuSet = 247;
        llInitFeatureSet2MPHY(TRUE);
        llInitFeatureSetDLE(TRUE);
        ATT_SetMTUSizeMax(mtuSet);
        LOG("[2Mbps | DLE | MTU %d] \n", mtuSet);
    }
    #else
		ATT_SetMTUSizeMax(23);
		llInitFeatureSet2MPHY(FALSE);
		llInitFeatureSetDLE(FALSE);
    #endif
    ///< Setup a delayed profile startup
    osal_set_event( simpleBLEPeripheral_TaskID, SBP_START_DEVICE_EVT );
    ///< for receive HCI complete message
    GAP_RegisterForHCIMsgs(simpleBLEPeripheral_TaskID);

    #if(APP_CFG_RPA_TEST)
	///< ========================= For Resolving Private Address testing
	uint8 addrType = ADDRTYPE_PUBLIC;

	initResolvingList();
	GAPRole_SetParameter(GAPROLE_ADV_DIRECT_TYPE, 1, &addrType);
	GAPRole_SetParameter(GAPROLE_ADV_DIRECT_ADDR, LL_DEVICE_ADDR_LEN, peerAddrList[0]);
	LOG("======== RPA Init ========\n");
	#endif

    LL_PerStats_Init(&g_perStatsByChanTest);

    LOG("======================SimpleBLEPeripheral_Init Done====================\n");
}

/**
 * @fn		uint16 SimpleBLEPeripheral_ProcessEvent( uint8 task_id, uint16 events )
 * @brief	Simple BLE Peripheral Application Task event processor.  This function
 *          is called to process all events for the task.  Events
 *          include timers, messages and any other user defined events.
 * @param	task_id - The OSAL assigned task ID.
 * @param   events  - events to process.  This is a bit map and can
 *                    contain more than one event.
 * @return	events not processed
 */
uint16 SimpleBLEPeripheral_ProcessEvent( uint8 task_id, uint16 events )
{
    VOID task_id;	///< OSAL required parameter that isn't used in this function

    if ( events & SYS_EVENT_MSG )
    {
        uint8 *pMsg;

        if ( (pMsg = osal_msg_receive( simpleBLEPeripheral_TaskID )) != NULL )
        {
            simpleBLEPeripheral_ProcessOSALMsg( (osal_event_hdr_t *)pMsg );

            ///< Release the OSAL message
            VOID osal_msg_deallocate( pMsg );
        }

        ///< return unprocessed events
        return (events ^ SYS_EVENT_MSG);
    }

    if ( events & SBP_START_DEVICE_EVT )
    {
        ///< Start the Device
        VOID GAPRole_StartDevice( &simpleBLEPeripheral_PeripheralCBs );
    #if(DEFAULT_GAPBOND_MGR_ENABLE==1)
        // Start Bond Manager, 2017-11-15
		LOG("bond\r\n");
        VOID GAPBondMgr_Register( &simpleBLEPeripheral_BondMgrCBs );
    #endif
        // Set timer for first periodic event
        osal_start_timerEx( simpleBLEPeripheral_TaskID, SBP_PERIODIC_EVT, SBP_PERIODIC_EVT_PERIOD );
        
        HCI_LE_ReadResolvingListSizeCmd();

        return ( events ^ SBP_START_DEVICE_EVT );
    }
	
	if( events & SBP_PERIODIC_EVT )
	{
		osal_start_timerEx( simpleBLEPeripheral_TaskID, SBP_PERIODIC_EVT, SBP_PERIODIC_EVT_PERIOD );
		if(notifyByteCnt !=0 || writeByteCnt != 0)
		{
			LOG("Up:%d,Down:%d", notifyByteCnt, writeByteCnt);
		}
		notifyByteCnt = 0;
		writeByteCnt = 0;
		return ( events ^ SBP_PERIODIC_EVT );
	}

    ///< enable adv
    if ( events & SBP_RESET_ADV_EVT )
    {
        uint8 initial_advertising_enable = TRUE;
		
        GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &initial_advertising_enable );	
	  
        return ( events ^ SBP_RESET_ADV_EVT );
    }  

    ///< notifity
    if ( events & SBP_NOTIFY_TEST_EVT )
    {
        for(int i=0;i<notifyPktNum;i++)
        {
            notifyBuf[0]=HI_UINT16(notifyCnt);
            notifyBuf[1]=LO_UINT16(notifyCnt);

            if(i==0)
            {
                uint16 connIntv;
                uint16 connlatency;
                GAPRole_GetParameter(GAPROLE_CONN_INTERVAL,&connIntv);
                GAPRole_GetParameter(GAPROLE_CONN_LATENCY, &connlatency);
                
                int8 rssi;      LL_ReadRssi(0,&rssi);
                uint16 foff;    LL_ReadFoff(0,&foff);
                uint8 carrSens; LL_ReadCarrSens(0, &carrSens);
                notifyBuf[17]=rssi;
                notifyBuf[18]=0xff&((foff-512)>>2);	///< 4KHz resolution
                notifyBuf[19]=carrSens;
            }
			uint8 len = ATT_GetCurrentMTUSize(0)-3;
            uint8 status= simpleProfile_Notify(SIMPLEPROFILE_CHAR6,len,notifyBuf);
            
            if(SUCCESS==status)
            {             
                notifyCnt++;
				notifyByteCnt+=len;
            }
            else
            {
                break;
            }
        }
		if(connEvtEndNotify == 0)
		{
			if(notifyInterval>0)
			{
				osal_start_timerEx( simpleBLEPeripheral_TaskID, SBP_NOTIFY_TEST_EVT, notifyInterval );
			}
			else
			{
				osal_stop_timerEx( simpleBLEPeripheral_TaskID, SBP_NOTIFY_TEST_EVT );
				notifyCnt=0;
			}
		}
        return ( events ^ SBP_NOTIFY_TEST_EVT );
    }
    // Discard unknown events
    return 0;
}

/**
 * @fn		void simpleBLEPeripheral_ProcessOSALMsg( osal_event_hdr_t *pMsg )
 * @brief	Process an incoming task message.
 * @param	pMsg - message to process
 * @return	none
 */
static void simpleBLEPeripheral_ProcessOSALMsg( osal_event_hdr_t *pMsg )
{
    hciEvt_CmdComplete_t *pHciMsg;
    
    switch ( pMsg->event )
    {  
        case HCI_GAP_EVENT_EVENT:
        {
            
            switch( pMsg->status )
            {
                case HCI_COMMAND_COMPLETE_EVENT_CODE:
                    pHciMsg = (hciEvt_CmdComplete_t *)pMsg;
                
                    LOG("==> HCI_COMMAND_COMPLETE_EVENT_CODE: %x\n", pHciMsg->cmdOpcode);
                    //safeToDealloc = gapProcessHCICmdCompleteEvt( (hciEvt_CmdComplete_t *)pMsg );
                break;


                default:
                    //safeToDealloc = FALSE;  ///< Send to app
                break;
            }
        }
      }

}

/**
 * @fn		void peripheralStateReadRssiCB( int8  rssi )
 * @brief	Notification from the profile of a state change.
 * @param	rssi
 * @return	none
 */
static void peripheralStateReadRssiCB( int8  rssi )
{
//    notifyBuf[15]++;
//    notifyBuf[16]=rssi;
//    notifyBuf[17]=HI_UINT16(g_conn_param_foff);
//    notifyBuf[18]=LO_UINT16(g_conn_param_foff);;
//    notifyBuf[19]=g_conn_param_carrSens;
}

/**
 * @fn		void peripheralStateNotificationCB( gaprole_States_t newState )
 * @brief	Notification from the profile of a state change.
 * @param	newState - new state
 * @return	none
 */
static void peripheralStateNotificationCB( gaprole_States_t newState )
{
    switch ( newState )
    {
        case GAPROLE_STARTED:
        {
            uint8 ownAddress[B_ADDR_LEN];
            uint8 str_addr[14]={0}; 
            uint8 initial_advertising_enable = FALSE;
        
            GAPRole_GetParameter(GAPROLE_BD_ADDR, ownAddress);
            #if(0) 
            uint8 systemId[DEVINFO_SYSTEM_ID_LEN];
            ///< use 6 bytes of device address for 8 bytes of system ID value
            systemId[0] = ownAddress[0];
            systemId[1] = ownAddress[1];
            systemId[2] = ownAddress[2];
        
            ///< set middle bytes to zero
            systemId[4] = 0x00;
            systemId[3] = 0x00;
        
            ///< shift three bytes up
            systemId[7] = ownAddress[5];
            systemId[6] = ownAddress[4];
            systemId[5] = ownAddress[3];
        
            DevInfo_SetParameter(DEVINFO_SYSTEM_ID, DEVINFO_SYSTEM_ID_LEN, systemId);
            #endif    

            osal_memcpy(&str_addr[0],bdAddr2Str(ownAddress),14);
            osal_memcpy(&attDeviceName[9],&str_addr[6],8);

            ///< Set the GAP Characteristics
            GGS_SetParameter( GGS_DEVICE_NAME_ATT, GAP_DEVICE_NAME_LEN, attDeviceName );

            GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &initial_advertising_enable );


            #if(APP_CFG_RPA_TEST)
            ///< init RPA list and enable resolving address
            for (int i = 0; i < 3; i ++)
                HCI_LE_AddDevToResolvingListCmd(peerAddrType[i], peerAddrList[i], peerIrkList[i], localIrkList[i]);

			HCI_LE_SetResolvablePrivateAddressTimeoutCmd(60);

			HCI_LE_SetAddressResolutionEnableCmd(TRUE);
            #endif
            
			//osal_start_timerEx(simpleBLEPeripheral_TaskID, SBP_RESET_ADV_EVT, 500);    
			osal_set_event(simpleBLEPeripheral_TaskID, SBP_RESET_ADV_EVT);    
                                    
        }
            break;
        
        case GAPROLE_ADVERTISING:
        {
            osal_stop_timerEx(simpleBLEPeripheral_TaskID, SBP_NOTIFY_TEST_EVT);
            notifyCnt=0;
            notifyInterval = 0;
         }   
            break;
        
        case GAPROLE_CONNECTED:
            HCI_ConnEventDoneNoticeCmd(simpleBLEPeripheral_TaskID, NULL);
			notifyCnt=0;
            break;
        
        case GAPROLE_CONNECTED_ADV:
            break;      
        case GAPROLE_WAITING:
            break;
        
        case GAPROLE_WAITING_AFTER_TIMEOUT:
            break;
        
        case GAPROLE_ERROR:
            break;
        
        default:
            break;        
    }  
    gapProfileState = newState;

    LOG("[GAP ROLE %d]\n",newState);
    VOID gapProfileState;     
}

/**
 * @fn		void simpleProfileChangeCB( uint8 paramID, uint8 len )
 * @brief	Callback from SimpleBLEProfile indicating a value change
 * @param	paramID - parameter ID of the value that was changed.
			len - parameter length
 * @return	none
 */
static void simpleProfileChangeCB( uint8 paramID, uint8 len )
{
  uint8 newValue[IBEACON_ATT_LONG_PKT];
    
  switch( paramID )
  {
    case SIMPLEPROFILE_CHAR5:
      SimpleProfile_GetParameter( SIMPLEPROFILE_CHAR5, newValue );
      LOG("[WRT_ATT] %02x \n",newValue[0]);
      ///< 0xff reset to connectable adv
      if (newValue[0] == CMD_SET_ADV_IND_INTERVAL)
      {
         otaAdvIntv = newValue[1];
         LOG("[AdvIntv CONNECT] %04d\n",otaAdvIntv*5);
          ///< option:
          ///< 1. reset
          ///< 2. reset advertisement
          g_current_advType = LL_ADV_CONNECTABLE_UNDIRECTED_EVT;
          pGlobal_config[ADV_CHANNEL_INTERVAL] = 1400;//6250;
          VOID updateAdvData();		  
      }
      else if(newValue[0]==CMD_SET_ADV_NONCONN_IND_INTERVAL)
      {
         otaAdvIntv = newValue[1];
         LOG("[AdvIntv NONCONN] %04d\n",otaAdvIntv*5);
          // option:
          // 1. reset
          // 2. reset advertisement
          g_current_advType = LL_ADV_NONCONNECTABLE_UNDIRECTED_EVT;
          pGlobal_config[ADV_CHANNEL_INTERVAL] = 600;//6250;
          VOID updateAdvData();		  
      }
      //===============================================================================
      // check per stats
      else if(newValue[0]==CMD_PRINT_PER_INFO)
      {
		check_PerStatsProcess();	  
      }
      //===============================================================================
      // standby off mode 
      else if(newValue[0]==CMD_SLEEP)
      {
          pwroff_cfg_t cfg =
          {
            .pin = WAKEUP_PIN,
            .type = POL_FALLING,
            .on_time = 3000
          };
		  uint16 connIntv;
          GAPRole_GetParameter(GAPROLE_CONN_INTERVAL,&connIntv);
		  connIntv = ((connIntv<<2)+connIntv)>>2;//*1.25
		  GAPRole_TerminateConnection();
		  WaitMs(connIntv*6);
          if (newValue[1] == 0)
          {
              LOG("PWR OFF\n");
              hal_pwrmgr_poweroff(&cfg, 1);
              
          }
          else
          {
              cfg.on_time = 1000*newValue[1];
              LOG("STANDBY on time %d\n",cfg.on_time);
              hal_pwrmgr_enter_standby(&cfg,0);
          }
      }
      //===============================================================================
      // [0x00 a1 a2 ] : enable notifiy , notifiy intv is a1
      else if(newValue[0]== CMD_START_NOTIFY_TEST )
      {
        connEvtEndNotify = (newValue[1]&0x80)>>7;
        notifyInterval   = (newValue[1]&0x7f)*5;
        notifyPktNum     = newValue[2]; 
        
        uint16 connIntv;
        GAPRole_GetParameter(GAPROLE_CONN_INTERVAL,&connIntv);
        connIntv = ((connIntv<<2)+connIntv)>>2;//*1.25
		  
		if(connEvtEndNotify)
		{
			notifyInterval = connIntv;
			if(notifyPktNum==0)
            {
                notifyPktNum=1;
            }
			HCI_ConnEventDoneNoticeCmd(simpleBLEPeripheral_TaskID, SBP_NOTIFY_TEST_EVT);
			LOG("[Notf_Conn] p%d ni%d ci%d\n",notifyPktNum,notifyInterval,connIntv);
		}
		else
		{
			if(notifyInterval>0)
			{
				if(notifyPktNum==0)
				{
					notifyPktNum=1;
				}
				HCI_ConnEventDoneNoticeCmd(simpleBLEPeripheral_TaskID, NULL);
                osal_start_timerEx( simpleBLEPeripheral_TaskID, SBP_NOTIFY_TEST_EVT, notifyInterval );
			}
			else
			{
				HCI_ConnEventDoneNoticeCmd(simpleBLEPeripheral_TaskID, NULL);
				osal_stop_timerEx(simpleBLEPeripheral_TaskID, SBP_NOTIFY_TEST_EVT);
				notifyCnt=0;
			}
			LOG("[Notify   ] p%d ni%d ci%d\n",notifyPktNum,notifyInterval,connIntv);
		}
      }
      
      //===============================================================================
      // [0x01 a1 a2 a3 a4 ] : cont config ,a1 
      else if(newValue[0]==CMD_SET_CONN_PARAMETER)
      {
        otaConnIntvMin      = newValue[1];
        otaConnIntvMax      = newValue[2];
        otaConnIntvLatency  = newValue[3];
        otaConnTimeOut      = newValue[4];

        uint16 desired_min_interval = (otaConnIntvMin<<2)<6 ? 6:(otaConnIntvMin<<2);
        uint16 desired_max_interval = (otaConnIntvMax<<2)<6 ? 6:(otaConnIntvMax<<2);
        uint16 desired_slave_latency = otaConnIntvLatency;
        uint16 desired_conn_timeout = otaConnTimeOut*100;
        uint8 updateConnParams = true;

        LOG("[ConnPara] %04d %04d L%02d T%02d\n",desired_min_interval,
                                                 desired_max_interval,
                                                 desired_slave_latency,
                                                 desired_conn_timeout);

        GAPRole_SetParameter( GAPROLE_MIN_CONN_INTERVAL, sizeof( uint16 ), &desired_min_interval );
        GAPRole_SetParameter( GAPROLE_MAX_CONN_INTERVAL, sizeof( uint16 ), &desired_max_interval );
        GAPRole_SetParameter( GAPROLE_SLAVE_LATENCY, sizeof( uint16 ), &desired_slave_latency );
        GAPRole_SetParameter( GAPROLE_TIMEOUT_MULTIPLIER, sizeof( uint16 ), &desired_conn_timeout );

        GAPRole_SetParameter( GAPROLE_PARAM_UPDATE_REQ, sizeof( uint8 ), &updateConnParams );
      }
      //===============================================================================
      // [0x02 a1 a2] : max mtu size
      else if(newValue[0]==CMD_SET_MTU_SIZE_MAX)
      {
        if(newValue[1] > L2CAP_MTU_SIZE)
		{
			newValue[1] = L2CAP_MTU_SIZE;
		}
        g_ATT_MTU_SIZE_MAX = newValue[1];
        LOG("[MTU SIZE MAX] %02d\n",g_ATT_MTU_SIZE_MAX);
      }
      //===============================================================================
      // [0x03 a1 a2] :set pdu length  ,a1 is the pdulength
      else if(newValue[0]==CMD_SET_DLE_LENGTH)
      {
		uint8 pduLen = newValue[1];
        uint16 txTime = (pduLen+10+4)<<3;
 
        HCI_LE_SetDataLengthCmd(0,pduLen, txTime);
		LOG("[DLE] pduLen=%d,txTime=%d\n",pduLen,txTime);  

//        uint8 status=HCI_DateLengthChangedNoticeCmd(simpleBLEPeripheral_TaskID, SBP_DLE_CHANGE_EVT);
//        LOG_DEBUG("[DLE] %02d %02d %d\n",pduLen,txTime,status);
      }
      //===============================================================================
      // [0x05 a1 a2] :set phy mode 
      else if(newValue[0]==CMD_SET_PHY_MODE)
      {
        
        uint8 allPhy = newValue[1];
        uint8 txPhy  = newValue[2];
        uint8 defPhy  = newValue[3];


        if(defPhy>0)
        {
            HCI_LE_SetDefaultPhyMode(0,allPhy, defPhy, defPhy);
        }
        else
        {
            HCI_LE_SetDefaultPhyMode(0,allPhy, 0x03, 0x03);
        }

        HCI_LE_SetPhyMode(0,allPhy, txPhy,txPhy,0);//tx/rx set as same phy
		LOG("[PHY] allPhy=%d txPhy=%d\n",allPhy,txPhy);

//        uint8 status=HCI_PhyUpdateNoticeCmd(simpleBLEPeripheral_TaskID, SBP_PHY_UPDATE_EVT);
//        LOG_DEBUG("[PHY] %02d %02d %d\n",allPhy,txPhy,status);
      }
      break;
	  
	case SIMPLEPROFILE_CHAR7:
	  writeByteCnt+=len;
	  break;
    default:
      // not process other attribute change
      break;
  }
}


/**
 * @fn		updateAdvData
 * @brief	
 * @param	none
 * @return	none
 */
static void updateAdvData(void)
{
    uint8  new_uuid[IBEACON_UUID_LEN];
    uint16 major;
    uint16 minor;
    uint8  power;
    
    // 1. get the new setting from GATT attributes
    SimpleProfile_GetParameter( SIMPLEPROFILE_CHAR1, new_uuid );
    SimpleProfile_GetParameter( SIMPLEPROFILE_CHAR2, &major );
    SimpleProfile_GetParameter( SIMPLEPROFILE_CHAR3, &minor );
    SimpleProfile_GetParameter( SIMPLEPROFILE_CHAR4, &power );	  
    
    // 2. update adv data storage
    //set UUID
    VOID osal_memcpy(&advertData[9], new_uuid, IBEACON_UUID_LEN);
    // set major
    advertData[25] = LO_UINT16( major );
    advertData[26] = HI_UINT16( major );
    // set minor
    advertData[27] = LO_UINT16( minor );
    advertData[28] = HI_UINT16( minor );
    // set power
    advertData[29] = power;
    // 3. disconnect all connection
    GAPRole_TerminateConnection();
		
    // 4. close advert
    uint8 initial_advertising_enable = FALSE;		
    GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &initial_advertising_enable );		
        
    // 5. update adv data
    // 5.1 update adv type
    uint8 advType = g_current_advType;    
    GAPRole_SetParameter( GAPROLE_ADV_EVENT_TYPE, sizeof( uint8 ), &advType );	  

    uint16 advInt = otaAdvIntv<<3;
    GAP_SetParamValue( TGAP_LIM_DISC_ADV_INT_MIN, advInt );
    GAP_SetParamValue( TGAP_LIM_DISC_ADV_INT_MAX, advInt );
    GAP_SetParamValue( TGAP_GEN_DISC_ADV_INT_MIN, advInt );
    GAP_SetParamValue( TGAP_GEN_DISC_ADV_INT_MAX, advInt );

    // 5.2 update advert broadcast
    GAPRole_SetParameter( GAPROLE_ADVERT_DATA, sizeof( advertData ), advertData );	

    // 5.3 set TxPower
    g_rfPhyTxPower = (power - DEFAULT_IBEACON_POWER)+DEFAULT_TX_POWER;
	if(g_rfPhyTxPower > RF_PHY_TX_POWER_EXTRA_MAX)
	{
		g_rfPhyTxPower = RF_PHY_TX_POWER_EXTRA_MAX;
	}
    rf_phy_set_txPower(g_rfPhyTxPower);

    // 6. set reset advertisement event, note that GAP/LL will process close adv event in advance
    osal_start_timerEx(simpleBLEPeripheral_TaskID, SBP_RESET_ADV_EVT,500);
}

/**
 * @fn		char *bdAddr2Str( uint8 *pAddr )
 * @brief	Convert Bluetooth address to string. Only needed when
 *			LCD display is used.
 * @param	pAddr - Bluetooth address
 * @return	char - string
 */
char *bdAddr2Str( uint8 *pAddr )
{
  uint8       i;
  char        hex[] = "0123456789ABCDEF";
  static char str[B_ADDR_STR_LEN];
  char        *pStr = str;
  
  *pStr++ = '0';
  *pStr++ = 'x';
  
  // Start from end of addr
  pAddr += B_ADDR_LEN;
  
  for ( i = B_ADDR_LEN; i > 0; i-- )
  {
    *pStr++ = hex[*--pAddr >> 4];
    *pStr++ = hex[*pAddr & 0x0F];
  }
  
  *pStr = 0;
  
  return str;
}

#if(APP_CFG_RPA_TEST==1)
/**
 * @fn		void initResolvingList(void)
 * @brief	
 * @param	none
 * @return	none
 */
static void initResolvingList(void)
{
    int i;
	uint8 temp;

	for (i = 0; i < RESOLVING_LIST_ENTRY_NUM; i++)
	{
	    osal_memset(&peerIrkList[i], (i + 1), LL_ENC_IRK_LEN);
		osal_memset(&localIrkList[i], (i + 1), LL_ENC_IRK_LEN);
		
        temp = ((i + 1) << 4) | (i + 1);
		osal_memset(&peerAddrList[i], temp, LL_DEVICE_ADDR_LEN);
		peerAddrType[i] = LL_DEV_ADDR_TYPE_PUBLIC;      // LL_DEV_ADDR_TYPE_RANDOM
	}
}
#endif

/**
 * @fn		void check_PerStatsProcess(void)
 * @brief	
 * @param	none
 * @return	none
 */
void check_PerStatsProcess(void)
{
    perStats_t perStats;
    uint16_t perRxNumTotal=0;
    uint16_t perRxCrcErrTotal=0;
    uint16_t perTxNumTotal=0;
    uint16_t perTxAckTotal=0;
    uint16_t perRxToCntTotal=0;
    uint16_t perConnEvtTotal=0;

    LOG("[PER STATS Notify]\r");
    LOG("----- ch connN rxNum rxCrc rxToN txAck txRty \r");
    for(uint8 i=0;i<37;i++)
    {
        LL_PerStasReadByChn(i,&perStats);
        LOG("[PER] %02d %05d %05d %05d %05d %05d %05d\n",i,perStats.connEvtCnt,
                                                     perStats.rxNumPkts,
                                                     perStats.rxNumCrcErr,
                                                     perStats.rxToCnt,
                                                     perStats.TxNumAck,
                                                     perStats.txNumRetry);
        
        perConnEvtTotal+= perStats.connEvtCnt;
        perRxNumTotal+= perStats.rxNumPkts;
        perRxCrcErrTotal+= perStats.rxNumCrcErr;
        perRxToCntTotal+= perStats.rxToCnt;
        perTxAckTotal+= perStats.TxNumAck;
        perTxNumTotal+= perStats.txNumRetry;
                        
    }
    LOG("TOTAL ch connN rxNum rxCrc rxToN txAck txRty \r");
    LOG("\n[PER] -- %05d %05d %05d %05d %05d %05d\n",perConnEvtTotal,
                                                     perRxNumTotal,
                                                     perRxCrcErrTotal,
                                                     perRxToCntTotal,
                                                     perTxAckTotal,
                                                     perTxNumTotal);
    LL_PerStatsReset();

}



/*********************************************************************
*********************************************************************/
